"""vmc_py controller."""

from controller import Robot
import math
import numpy as np
import copy
import matplotlib.pyplot as plt
import csv
import ast

# 画图使用
p_xFd = list()
p_yFd = list()
p_zFd = list()
p_xBd = list()
p_yBd = list()
p_zBd = list()
p_xF = list()
p_yF = list()
p_zF = list()
p_xB = list()
p_yB = list()
p_zB = list()
fig = plt.figure()
# plt.ion()
ax = plt.axes(projection='3d')

# create the Robot instance.
robot = Robot()

# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())

# 机器人所有马达
#   0  1  2  3
#   LF RF RB LB
# 0 跨 ........
# 1 髋 ........
# 2 膝 ........
robot_motor = []  # LF RF RB LB
for i in range(4):
    robot_motor.append([])
# 左前足马达
robot_motor[0].append(robot.getDevice("LFJ0Motor"))
robot_motor[0].append(robot.getDevice("LFJ1Motor"))
robot_motor[0].append(robot.getDevice("LFJ2Motor"))
# 右前足马达
robot_motor[1].append(robot.getDevice("RFJ0Motor"))
robot_motor[1].append(robot.getDevice("RFJ1Motor"))
robot_motor[1].append(robot.getDevice("RFJ2Motor"))
# 右后足马达
robot_motor[2].append(robot.getDevice("RBJ0Motor"))
robot_motor[2].append(robot.getDevice("RBJ1Motor"))
robot_motor[2].append(robot.getDevice("RBJ2Motor"))
# 左后足马达
robot_motor[3].append(robot.getDevice("LBJ0Motor"))
robot_motor[3].append(robot.getDevice("LBJ1Motor"))
robot_motor[3].append(robot.getDevice("LBJ2Motor"))

# 机器人所有位置传感器
#   0  1  2  3
#   LF RF RB LB
# 0 跨 ........
# 1 髋 ........
# 2 膝 ........
robot_pos_sensor = []  # LF RF RB LB
for i in range(4):
    robot_pos_sensor.append([])
# 左前足位置传感器
robot_pos_sensor[0].append(robot.getDevice("LFJ0Sensor"))
robot_pos_sensor[0].append(robot.getDevice("LFJ1Sensor"))
robot_pos_sensor[0].append(robot.getDevice("LFJ2Sensor"))
# 右前足位置传感器
robot_pos_sensor[1].append(robot.getDevice("RFJ0Sensor"))
robot_pos_sensor[1].append(robot.getDevice("RFJ1Sensor"))
robot_pos_sensor[1].append(robot.getDevice("RFJ2Sensor"))
# 右后足位置传感器
robot_pos_sensor[2].append(robot.getDevice("RBJ0Sensor"))
robot_pos_sensor[2].append(robot.getDevice("RBJ1Sensor"))
robot_pos_sensor[2].append(robot.getDevice("RBJ2Sensor"))
# 左后足位置传感器
robot_pos_sensor[3].append(robot.getDevice("LBJ0Sensor"))
robot_pos_sensor[3].append(robot.getDevice("LBJ1Sensor"))
robot_pos_sensor[3].append(robot.getDevice("LBJ2Sensor"))

# 触地传感器
# LF RF RB LB
robot_touch_sensor = []
robot_touch_sensor.append(robot.getDevice("LF_Touch"))
robot_touch_sensor.append(robot.getDevice("RF_Touch"))
robot_touch_sensor.append(robot.getDevice("RB_Touch"))
robot_touch_sensor.append(robot.getDevice("LB_Touch"))

# inertial unit
IMU = robot.getDevice("inertial unit")
GPS = robot.getDevice("gps")
KeyBoard = robot.getKeyboard()
Gyro = robot.getDevice("gyro")
Gyro.enable(timestep)
state_emitter = robot.getDevice("emitter")
cmd_receiver = robot.getDevice("receiver")
cmd_receiver.enable(timestep)
# 类似宏定义
FL = 0
FR = 1
BR = 2
BL = 3


# 已测试
# ROll Pich Yaw
def get_IMU_Angle():
    data = IMU.getRollPitchYaw()
    # return ROll Pich Yaw
    return [data[0] * 180.0 / math.pi, data[1] * 180.0 / math.pi, data[2] * 180.0 / math.pi]


def get_IMU_Quat():
    data = IMU.getQuaternion()
    return data


def get_GPS_Pos():
    data = GPS.getValues()
    return data


def get_GPS_Vel():
    data = GPS.getSpeedVector()
    return data


# 已测试
def webot_device_init():
    # Camera.enable(timestep)
    KeyBoard.enable(timestep)
    # IMU使能
    IMU.enable(timestep)
    GPS.enable(timestep)
    for leg in range(4):
        # 跨关节锁定
        robot_motor[leg][0].setPosition(0)
        # 初始化使能接触传感器
        robot_touch_sensor[leg].enable(timestep)
        # 初始化位置传感器
        for joint in range(3):
            robot_pos_sensor[leg][joint].enable(timestep)


# 已测试
def set_motor_torque(leg, name, torque):
    max = 5
    if torque > max:
        # print("error torque >max=", torque)
        torque = max
    if torque < -max:
        # print("error torque >max=", torque)
        torque = -max
    robot_motor[leg][name].setTorque(torque)


# 顺序是 LF RF RB LB，单位是角度
def get_motor_angle(leg, name):
    angle = 0
    angle = robot_pos_sensor[leg][name].getValue()
    return angle * 180.0 / math.pi


def get_gyro():
    data = Gyro.getValues()
    return data


# 已测试
def get_all_motor_angle():
    temp_list = list()
    for leg in range(4):  # 遍历四条腿 每条腿三个关节
        temp_list.append([])
        for joint in range(3):
            temp_list[leg].append(get_motor_angle(leg, joint))
    return temp_list


# 已测试
def is_foot_touching(leg):
    return robot_touch_sensor[leg].getValue()


# 已测试
def all_is_foot_touching():
    temp_list = list()
    for leg in range(4):
        temp_list.append(robot_touch_sensor[leg].getValue())
    return temp_list


def set_motor_position(leg, name, angle):
    robot_motor[leg][name].setPosition(angle)


def GetRotationMatrix(theta_x, theta_y, theta_z):
    sx = np.sin(theta_x)
    cx = np.cos(theta_x)
    sy = np.sin(theta_y)
    cy = np.cos(theta_y)
    sz = np.sin(theta_z)
    cz = np.cos(theta_z)
    return np.array([
        [cy * cz, cz * sx * sy - cx * sz, sx * sz + cx * cz * sy],
        [cy * sz, cx * cz + sx * sy * sz, cx * sy * sz - cz * sz],
        [-sy, cy * sx, cx * cy]])


def eulerAngles2rotationMat(theta):
    """
    Calculates Rotation Matrix given euler angles.
    :param theta: 1-by-3 list [rx, ry, rz] angle in degree
    :return:
    RPY角，是ZYX欧拉角，依次 绕定轴XYZ转动[rx, ry, rz]
    """
    theta = [i * math.pi / 180.0 for i in theta]

    R_x = np.array([[1, 0, 0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0, math.sin(theta[0]), math.cos(theta[0])]
                    ])

    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0, 1, 0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]
                    ])

    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]), 0],
                    [0, 0, 1]
                    ])
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R


def all_motor_stop():
    temp_list = get_all_motor_angle()
    for i in range(4):
        for j in range(3):
            set_motor_position(i, j, temp_list[i][j] / 180 * math.pi)


def quaternion_to_rotation_matrix(q):  # x, y ,z ,w
    rot_matrix = np.array(
        [[1.0 - 2 * (q[1] * q[1] + q[2] * q[2]), 2 * (q[0] * q[1] - q[3] * q[2]), 2 * (q[3] * q[1] + q[0] * q[2])],
         [2 * (q[0] * q[1] + q[3] * q[2]), 1.0 - 2 * (q[0] * q[0] + q[2] * q[2]), 2 * (q[1] * q[2] - q[3] * q[0])],
         [2 * (q[0] * q[2] - q[3] * q[1]), 2 * (q[1] * q[2] + q[3] * q[0]), 1.0 - 2 * (q[0] * q[0] + q[1] * q[1])]])
    return rot_matrix


class Quadruped_robot_mix():
    def __init__(self):
        self.Fst = 0
        self.Fsw = 1
        self.Bst = 2
        self.Bsw = 3

        self.fall_flag = False
        self.first_run_flag = True

        self.zh = 0.1  # 抬腿到高度 0.3
        self.H_des = -0.3  # 0.5
        self.Tf = 0.3  # 飞行时间
        self.Ts = 0.3  # 支撑时间
        # 机器人机身参数
        self.l = 0.2 + 0.043 * 2  # 前后髋关节w
        self.w = 0.105  # 左右髋关节
        self.a0 = 0.0754  # 髋部连杆长度
        self.a1 = 0.2  # 大腿连杆长度
        self.a2 = 0.2  # 小腿连杆长度
        self.M = 2  # + 8 * 0.25  # 躯干质量

        ###############需要调节的参数一览#####################
        # 使用GPS的支撑相系数
        self.ST_Kr = 1
        self.ST_Br = 1
        self.ST_Kz = 560
        self.ST_Bz = 10
        self.ST_Bx = 1
        self.ST_By = 1
        self.ST_Kw = 2
        self.ST_Bw = 1
        # 摆动相VMC参数
        self.SW_Kx = 1
        self.SW_Dx = 1
        self.SW_Ky = 1
        self.SW_Dy = 1
        self.SW_Kz = 1
        self.SW_Dz = 1
        self.Kvx = 2
        self.Kvy = 0.005

        ###############需要调节的参数一览#####################
        # 期望值
        self.Vx_des = 0
        self.Vy_des = 0
        self.Wz_des = 0
        self.Yaw_des = 0
        # 实际根据GPS得到的机身前向速度，侧向速度和yaw角转速,需要旋转矩阵
        self.Vx = 0
        self.Vy = 0
        self.Wz = 0
        self.h = [0, 0]
        # 根据足端或者IMU估计的的机身前向速度，侧向速度和yaw角转速
        self.est_Vx = 0
        self.est_Vy = 0
        self.est_Wz = 0

        # self.Pitch_des = - 0 / 180.0 * math.pi  # 上负
        # self.Roll_des = 0 / 180.0 * math.pi  # 左负
        # self.Yaw_des = 0

        # 存储变量
        # 当前欧拉角
        self.Pitch = 0
        self.Roll = 0
        self.Yaw = 0

        self.Quat = [0, 0, 0, 0]
        # 上一时刻
        self.pre_eulerAngle = [0, 0, 0]
        self.pre_pi_angle = 0
        self.pre_h = [-0.32, -0.32]  # 初始的悬空高度，需要修改

        self.pre_F_toe = [0, 0, 0]
        self.pre_B_toe = [0, 0, 0]
        self.pre_alpha = 0

        self.pre_body_pos = [0, 0, 0]

        self.pre_vx = 0
        self.pre_vy = 0
        self.pre_wz = 0

        self.pre_Ftoed = [0, 0, 0]
        self.pre_Btoed = [0, 0, 0]
        # 摆动足的中间变量
        self.toe0 = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]
        self.dtoe0 = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]

        # 欧拉角导数
        self.dot_Pitch = 0
        self.dot_Roll = 0
        self.dot_Yaw = 0
        # 上一时刻欧拉角导数
        self.dot_Pitch_pre = 0
        self.dot_Roll_pre = 0
        self.dot_Yaw_pre = 0
        # 四脚着地状态 逆时针编号
        self.is_touching = [0, 0, 0, 0]
        # 四足足端当前坐标
        self.pos = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]
        # 四足关节角，有12个
        self.theta = list()
        # 四足足端当前导数
        self.dot_pos = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]
        # 四足足端上一时坐标
        self.pre_pos = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]
        # 步态时间
        self.time = 0.0

    # 机身腿部坐标转换到单腿坐标下，是机身坐标，不是世界系坐标
    def convert_pos(self, leg, pos):
        x, y, z = pos
        if leg == 0 or leg == 1:
            x -= self.l / 2
        else:
            x += self.l / 2

        # if leg == 0 or leg == 3:
        #     y -= self.w / 2
        # else:
        #     y += self.w / 2
        return [x, y, z]

    # 这里出来的坐标是机身坐标系，搞清楚电机转的顺序，这里都是顺时针为正转，腿的顺序，腿的顺序LF RF RB LB
    def forwardkinematics(self, theta):
        a0 = self.a0  # 腿长
        a1 = - self.a1  # 腿长
        a2 = - self.a2  # 腿长

        flag = [1.0, -1.0, -1.0, 1.0]
        # 这个和腿的顺序有关
        flag_d = [1.0, 1.0, -1.0, -1.0]
        # 这个和腿的顺序有关
        flag_l = [1.0, -1.0, -1.0, 1.0]
        # 足底位置

        temp_toe = list()
        for legIdx in range(len(theta)):
            s0 = math.sin(theta[legIdx][0] / 180.0 * math.pi)
            c0 = math.cos(theta[legIdx][0] / 180.0 * math.pi)
            s1 = math.sin(theta[legIdx][1] / 180.0 * math.pi)
            c1 = math.cos(theta[legIdx][1] / 180.0 * math.pi)

            s12 = math.sin(theta[legIdx][1] / 180.0 * math.pi + theta[legIdx][2] / 180.0 * math.pi)
            c12 = math.cos(theta[legIdx][1] / 180.0 * math.pi + theta[legIdx][2] / 180.0 * math.pi)

            temp_toe.append([])

            x = a1 * s1 + a2 * s12 + flag_d[legIdx] * self.l / 2
            y = flag[legIdx] * a0 * c0 - a1 * c1 * s0 - a2 * c12 * s0 + flag_l[legIdx] * self.w / 2
            z = flag[legIdx] * a0 * s0 + a1 * c1 * c0 + a2 * c12 * c0

            temp_toe[legIdx].append(x)
            temp_toe[legIdx].append(y)
            temp_toe[legIdx].append(z)

        # print(temp_toe)
        return temp_toe

    # 只传入一个腿的三个关节值
    def create_transJ(self, theta, leg):
        a0 = self.a0
        a1 = - self.a1  # 腿长
        a2 = - self.a2  # 腿长

        flag = [1.0, -1.0, -1.0, 1.0]

        s0 = math.sin(theta[0] / 180.0 * math.pi)
        c0 = math.cos(theta[0] / 180.0 * math.pi)
        s1 = math.sin(theta[1] / 180.0 * math.pi)
        c1 = math.cos(theta[1] / 180.0 * math.pi)

        s12 = math.sin(theta[1] / 180.0 * math.pi + theta[2] / 180.0 * math.pi)
        c12 = math.cos(theta[1] / 180.0 * math.pi + theta[2] / 180.0 * math.pi)

        transJ = np.zeros((3, 3))
        # 分别求偏导数
        transJ[0, 0] = 0
        transJ[0, 1] = a1 * c1 + a2 * c12
        transJ[0, 2] = a2 * c12

        transJ[1, 0] = -flag[leg] * a0 * s0 - (a1 * c1 + a2 * c12) * c0
        transJ[1, 1] = s0 * (a1 * s1 + a2 * s12)
        transJ[1, 2] = a2 * s0 * s12

        transJ[2, 0] = - s0 * (a1 * c1 + a2 * c12) + flag[leg] * a0 * c0
        transJ[2, 1] = - c0 * (a1 * s1 + a2 * s12)
        transJ[2, 2] = - a2 * c0 * s12
        return transJ.T

    def update_food_touch_sensor(self):
        self.is_touching = all_is_foot_touching()

    def update_theta(self):
        self.theta = get_all_motor_angle()

    # 相位切换, 通过时间和足底传感器确定支撑对角腿和摆动对角腿
    def phase_swap(self):
        if self.time > 0.75 * self.Tf:
            if self.Fst == FL:
                if self.is_touching[FR] == 1 and self.is_touching[BL] == 1:
                    self.Fst = FR
                    self.Fsw = FL
                    self.Bst = BL
                    self.Bsw = BR
                    self.time = 0
            else:
                if self.is_touching[FL] == 1 and self.is_touching[BR] == 1:
                    self.Fst = FL
                    self.Fsw = FR
                    self.Bst = BR
                    self.Bsw = BL
                    self.time = 0

    def body_speed_euler(self):
        speed_v = get_GPS_Vel()
        # print("speed_v:"+str(speed_v))
        self.Wz = self.dot_Yaw
        speed = eulerAngles2rotationMat([self.Roll, self.Pitch, self.Yaw]).transpose() @ speed_v
        self.Vx = speed[0]
        self.Vy = speed[1]
        print("speed:" + str(speed))
        # print(self.Pitch,self.Roll,self.Yaw)

    def body_speed_quat(self):
        speed_v = get_GPS_Vel()
        self.Wz = self.dot_Yaw
        speed = quaternion_to_rotation_matrix(self.Quat).transpose() @ speed_v
        self.Vx = speed[0]
        self.Vy = speed[1]
        # print(self.Wz)
        # print(self.Vy)
        # print("speed:"+str(speed))

    def handle_receiver(self):
        if cmd_receiver.getQueueLength() > 0:
            received_data_string = cmd_receiver.getString()
            cmd_vel = ast.literal_eval(received_data_string)

            print(cmd_vel)
            cmd_receiver.nextPacket()

    def handle_emitter(self):
        gps_value = get_GPS_Pos()  # [x,y,z]
        gps_speed_vec = get_GPS_Vel()  # [vx,vy,vz]
        state_vec = str(gps_value + gps_speed_vec)
        message = state_vec.encode("utf-8")
        # print(message)
        state_emitter.send(message)

    # 更新全部状态
    def update_robot_state(self):
        self.key_control1()
        # 更新IMU及导数
        self.update_IMU()
        # 足底接触传感器更新
        self.update_food_touch_sensor()
        # 换相
        self.phase_swap()
        # 更新关节角
        self.update_theta()
        # 更新四条腿的运动学正解
        self.pos = self.forwardkinematics(self.theta)
        # 跟新足底速度
        self.update_V()
        # 直接利用GPS获取机身的位置
        self.body_speed_quat()
        # 发送和接受指令
        self.handle_receiver()
        self.handle_emitter()

        self.time += timestep * 0.001

    # 计算落足点,这个要在读取所有数据后面,这个是世界坐标系下
    def calFootPos(self, legID, vxyGoalGlobal, dYawGoal, phase):
        # 落足点的P参数
        _kx = 0.005
        _ky = 0.005
        _kyaw = 0.005

        _nextStep = [0, 0, 0]
        _bodyVelGlobal = get_GPS_Vel()
        _bodyWGlobal = quaternion_to_rotation_matrix(self.Quat) @ get_gyro()

        _nextStep[0] = _bodyVelGlobal[0] * (1 - phase) * self.Tf + _bodyVelGlobal[0] * self.Ts / 2 + _kx * (
                _bodyVelGlobal[0] - vxyGoalGlobal[0])
        _nextStep[1] = _bodyVelGlobal[1] * (1 - phase) * self.Tf + _bodyVelGlobal[1] * self.Ts / 2 + _ky * (
                _bodyVelGlobal[1] - vxyGoalGlobal[1])
        _nextStep[2] = 0  # 落足点z轴是0

        _nextYaw = self.Yaw * (1 - phase) * self.Tf + self.dot_Yaw * self.Ts / 2 + _kyaw * (dYawGoal - self.dot_Yaw)

        # _nextStep[0] += _feetRadius(legID) * np.cos(self.Yaw + _feetInitAngle(legID) + _nextYaw)
        # _nextStep[1] += _feetRadius(legID) * np.sin(self.Yaw + _feetInitAngle(legID) + _nextYaw)

        _footPos = get_GPS_Pos() + _nextStep
        _footPos[2] = 0.0

        return _footPos

    # IMU获得欧拉角，导数做低通滤波，滤过震动
    def update_IMU(self):
        eulerAngle = get_IMU_Angle()  # Roll Pitch Yaw 当前欧拉角
        # print("roll pitch yaw",eulerAngle)

        self.Roll = eulerAngle[0]  # 获得当前roll
        self.dot_Roll = (eulerAngle[0] - self.pre_eulerAngle[0]) / (0.001 * timestep)  # 求导数 dot_roll
        self.dot_Roll = self.dot_Roll * 0.3 + self.dot_Roll_pre * 0.7

        self.Pitch = eulerAngle[1]  # 获得当前pitch
        self.dot_Pitch = (eulerAngle[1] - self.pre_eulerAngle[1]) / (0.001 * timestep)  # 求导数 dot_pitch
        self.dot_Pitch = self.dot_Pitch * 0.3 + self.dot_Pitch_pre * 0.7  # 低通滤波

        self.Yaw = eulerAngle[2]  # 获得当前yaw
        self.dot_Yaw = get_gyro()[2]
        # self.dot_Yaw = (eulerAngle[2] - self.pre_eulerAngle[2]) / (0.001 * timestep)  # 求导数 dot_yaw
        self.dot_Yaw = self.dot_Yaw * 0.3 + self.dot_Yaw_pre * 0.7  # 低通滤波

        if self.first_run_flag == True:
            self.dot_Yaw = 0
            self.dot_Pitch = 0
            self.dot_Roll = 0

        self.dot_Roll_pre = self.dot_Roll  # 更新记录上次的变量 dot_Roll_pre
        self.dot_Pitch_pre = self.dot_Pitch  # 更新记录上次的变量 dot_Pitch_pre
        self.dot_Yaw_pre = self.dot_Yaw  # 更新记录上次的变量 dot_Yaw_pre

        self.Quat = get_IMU_Quat()
        self.pre_eulerAngle = copy.deepcopy(eulerAngle)

    # 所有速度速度更新
    def update_V(self):
        if self.first_run_flag == True:
            self.first_run_flag = False
        else:
            dot_pos = (((np.array(self.pos) - np.array(self.pre_pos))
                        / (0.001 * timestep)) * 1.0).tolist()
            self.dot_pos = copy.deepcopy(dot_pos)
        # 上一步坐标
        self.pre_pos = copy.deepcopy(self.pos)

    # 缓慢叠加数值
    def acc(self, acc_value, dot, vel):
        value = dot
        if (dot < vel):
            value += acc_value
            if (dot > vel):
                value = vel
        else:
            value -= acc_value
            if (dot < vel):
                value = vel
        return value

    def key_control1(self):
        key = KeyBoard.getKey()
        if key == ord('W'):
            self.Vx_des = self.acc(0.001, self.Vx_des, 0.5)

            # self.Vx_des = 0.8
        elif key == ord('S'):
            # self.Vx_des = -0.8
            self.Vx_des = self.acc(0.0005, self.Vx_des, -0.5)

        elif key == ord('A'):
            # self.Wz_des = 15
            self.Wz_des = self.acc(0.0005, self.Wz_des, 1.57 * 2)

        elif key == ord('D'):
            # self.Wz_des = -15
            self.Wz_des = self.acc(0.0005, self.Wz_des, -1.57 * 2)
        else:
            self.Vx_des = 0
            self.Wz_des = 0

    # 这里是摆线的轨迹,这里是世界坐标系下的
    def curve(self, xt, yt, init_pos):
        ft = [0, 0, 0]  # x,z
        dft = [0, 0, 0]  # x,z
        xT = xt
        yT = yt
        x0 = init_pos[0]
        y0 = init_pos[1]
        z0 = init_pos[2]
        Tf = self.Tf
        t = self.time / Tf
        zh = self.zh
        phasePI = 2 * np.pi * t
        ft[0] = (xT - x0) * (phasePI - np.sin(phasePI)) / (2 * np.pi) + x0
        ft[1] = (yT - y0) * (phasePI - np.sin(phasePI)) / (2 * np.pi) + y0
        ft[2] = zh * (1 - np.cos(phasePI)) / 2 + z0
        dft[0] = (xT - x0) * (1 - np.cos(phasePI)) / Tf
        dft[1] = (yT - y0) * (1 - np.cos(phasePI)) / Tf
        dft[2] = zh * np.pi * np.sin(phasePI) / Tf
        return [ft, dft]

    def control(self):
        # *摔倒时，停止控制
        if math.fabs(self.Roll) >= 30:
            self.fall_flag = True
        if self.fall_flag:
            all_motor_stop()
            # plt.ioff()
            # plt.show()
            return
        # 读取摆动和支持相的足底坐标
        if self.Fst == FL:
            stF_toe = self.pos[FL]
            stB_toe = self.pos[BR]
            swF_toe = self.pos[FR]
            swB_toe = self.pos[BL]
        else:
            stF_toe = self.pos[FR]
            stB_toe = self.pos[BL]
            swF_toe = self.pos[FL]
            swB_toe = self.pos[BR]
        # 支持相控制----------------------------------------
        dh = [0, 0]
        # 求每条支撑腿的高度,这里是腿部坐标系

        # h_des = [self.H_des + self.l / 2.0 * np.sin(self.Pitch / 180.0 * math.pi),
        #          self.H_des - self.l / 2.0 * np.sin(self.Pitch / 180.0 * math.pi)]
        h_des = [self.H_des,
                 self.H_des]
        self.h = [stF_toe[2], stB_toe[2]]
        dh[0] = (self.h[0] - self.pre_h[0]) / (0.001 * timestep)
        dh[1] = (self.h[1] - self.pre_h[1]) / (0.001 * timestep)
        self.pre_h = self.h

        F = np.zeros((6, 1))
        # 求支撑力，两条腿，符号不一样
        Gx_ff = self.M * 9.8 * math.sin(self.Pitch / 180.0 * math.pi)
        Gy_ff = self.M * 9.8 * math.sin(self.Roll / 180.0 * math.pi)
        Gz_ff = self.M * 9.8 * math.cos(self.Pitch / 180.0 * math.pi) * math.cos(self.Roll / 180.0 * math.pi)

        if self.Fst == FL or self.Fst == BL:
            F_n_sign = -1
            B_n_sign = 1
        else:
            F_n_sign = 1
            B_n_sign = -1

        F[0][0] = self.ST_Bx * (self.Vx_des - self.Vx) + F_n_sign * (
                self.ST_Kw * (self.Yaw_des - self.Yaw) + self.ST_Bw * (0 - self.Wz)) + Gx_ff / 2

        F[1][0] = self.ST_By * (self.Vy_des - self.Vy) + Gy_ff / 2

        F[2][0] = self.ST_Kz * (h_des[0] - self.h[0]) + self.ST_Bz * (0 - dh[0]) + F_n_sign * (
                self.ST_Kr * (0 - self.Roll) + self.ST_Br * (0 - self.dot_Roll)) + Gz_ff / 2

        F[3][0] = self.ST_Bx * (self.Vx_des - self.Vx) + B_n_sign * (
                self.ST_Kw * (self.Yaw_des - self.Yaw) + self.ST_Bw * (0 - self.Wz)) + Gx_ff / 2

        F[4][0] = self.ST_By * (self.Vy_des - self.Vy) + Gy_ff / 2

        F[5][0] = self.ST_Kz * (h_des[1] - self.h[1]) + self.ST_Bz * (0 - dh[1]) + B_n_sign * (
                self.ST_Kr * (0 - self.Roll) + self.ST_Br * (0 - self.dot_Roll)) + Gz_ff / 2

        tau_left = self.create_transJ(self.theta[self.Fst], self.Fst)
        tau_right = self.create_transJ(self.theta[self.Bst], self.Bst)
        temp_F = np.block([
            [tau_left, np.zeros((3, 3))],
            [np.zeros((3, 3)), tau_right]
        ])
        tau = - temp_F @ F
        # print(F)

        print(tau)
        # with open('tau.csv','a',newline='') as file:
        #     writer = csv.writer(file)
        # writer.writerow([self.Vx,self.Vy,self.Wz])
        # writer.writerow([Tx])
        # writer.writerow([tau[0],tau[1],tau[2]])
        # writer.writerow([pi_angle,dot_pi_angle])
        # writer.writerow([stF_toe[2],stB_toe[2],stF_toe[0],stB_toe[0]])

        if self.Fst == FL:
            set_motor_torque(FL, 0, tau[0][0])
            set_motor_torque(FL, 1, tau[1][0])
            set_motor_torque(FL, 2, tau[2][0])

            set_motor_torque(BR, 0, tau[3][0])
            set_motor_torque(BR, 1, tau[4][0])
            set_motor_torque(BR, 2, tau[5][0])
        else:
            set_motor_torque(FR, 0, tau[0][0])
            set_motor_torque(FR, 1, tau[1][0])
            set_motor_torque(FR, 2, tau[2][0])

            set_motor_torque(BL, 0, tau[3][0])
            set_motor_torque(BL, 1, tau[4][0])
            set_motor_torque(BL, 2, tau[5][0])
        # -----------------------------------------------------------------摆动相控制

        if self.Fsw == FL:
            swF_toe = self.convert_pos(FL, swF_toe)
            swB_toe = self.convert_pos(BR, swB_toe)
        else:
            swF_toe = self.convert_pos(FR, swF_toe)
            swB_toe = self.convert_pos(BL, swB_toe)
        #
        # print(swF_toe)
        # print(swB_toe)

        # 这里记录是摆动足的起始位置和起始速度，转化到单腿坐标系下，
        if self.time == 0.001 * timestep:
            self.toe0[0] = self.pos[self.Fsw]
            self.toe0[1] = self.pos[self.Bsw]
            if self.Fst == FL:
                self.toe0[0] = self.convert_pos(FR, self.toe0[0])
                self.toe0[1] = self.convert_pos(BL, self.toe0[1])
            else:
                self.toe0[0] = self.convert_pos(FL, self.toe0[0])
                self.toe0[1] = self.convert_pos(BR, self.toe0[1])

            self.dtoe0[0] = self.dot_pos[self.Fsw]
            self.dtoe0[1] = self.dot_pos[self.Bsw]

            self.dtoe0 = self.dot_pos

        # 在单腿坐标系下计算落足点
        xT = self.Vx_des * self.Ts / 2.0
        # xT = self.Vx_des * self.Ts / 2.0 + self.Kvx * (self.Vx - self.Vy_des)
        yT = self.Vy * self.Ts / 2.0 + self.Kvy * (self.Vy - self.Vy_des)  # + \
        # 0.005 * (self.toe_err_sum[self.Fsw] + self.toe_err_sum[self.Bsw])
        # self.Yaw * (1 - self.time) * self.Tf + self.dot_Yaw * self.Ts / 2 + _kyaw * (dYawGoal - self.dot_Yaw)
        Fpydes = yT
        Bpydes = yT
        if self.Fsw == FL:
            Fpydes = yT + self.w / 2 + self.a0 / 2
            Bpydes = yT - self.w / 2 - self.a0 / 2
        else:
            Fpydes = yT - self.w / 2 - self.a0 / 2
            Bpydes = yT + self.w / 2 + self.a0 / 2

        Ftoed, Fdtoed = self.curve(xT, Fpydes, self.toe0[0])
        Btoed, Bdtoed = self.curve(xT, Bpydes, self.toe0[1])

        if self.time > 0.85 * self.Tf:
            if self.is_touching[self.Fsw]:
                Ftoed, Fdtoed = self.pre_Ftoed, [0, 0, 0]

            if self.is_touching[self.Bsw]:
                Btoed, Bdtoed = self.pre_Btoed, [0, 0, 0]
        self.pre_Ftoed = Ftoed
        self.pre_Btoed = Btoed

        # with open('Ftoed.csv','a',newline='') as file:
        #     writer = csv.writer(file)
        #     writer.writerow([yT])

        # print(xT)
        # p_xF.append(swF_toe[1])
        # p_yF.append(swF_toe[0])
        # p_zF.append(swF_toe[2])
        #
        # p_xB.append(swB_toe[1])
        # p_yB.append(swB_toe[0])
        # p_zB.append(swB_toe[2])
        #
        # p_xFd.append(Ftoed[1])
        # p_yFd.append(Ftoed[0])
        # p_zFd.append(Ftoed[2])
        #
        # p_xBd.append(Btoed[1])
        # p_yBd.append(Btoed[0])
        # p_zBd.append(Btoed[2])
        #
        # ax.plot3D(p_xF, p_yF, p_zF, c="blue")
        # ax.plot3D(p_xFd, p_yFd, p_zFd, c="red")
        # ax.plot3D(p_xB, p_yB, p_zB, c="yellow")
        # ax.plot3D(p_xBd, p_yBd, p_zBd, c="red")

        # 求摆动相力,t=J^T*F

        F_sw = np.zeros((6, 1))

        F_sw[0][0] = self.SW_Kx * (Ftoed[0] - swF_toe[0]) + self.SW_Dx * (
                Fdtoed[0] - self.dot_pos[self.Fsw][0])

        F_sw[1][0] = self.SW_Ky * (Ftoed[1] - swF_toe[1]) + self.SW_Dy * (
                Fdtoed[1] - self.dot_pos[self.Fsw][1])

        F_sw[2][0] = self.SW_Kz * (Ftoed[2] - swF_toe[2]) + self.SW_Dz * (Fdtoed[2] - self.dot_pos[self.Fsw][2])

        F_sw[3][0] = self.SW_Kx * (Btoed[0] - swB_toe[0]) + self.SW_Dx * (
                Bdtoed[0] - self.dot_pos[self.Bsw][0])

        F_sw[4][0] = self.SW_Ky * (Btoed[1] - swB_toe[1]) + self.SW_Dy * (
                Bdtoed[1] - self.dot_pos[self.Bsw][1])

        F_sw[5][0] = self.SW_Kz * (Btoed[2] - swB_toe[2]) + self.SW_Dz * (Bdtoed[2] - self.dot_pos[self.Bsw][2])

        tau_sw_left = self.create_transJ(self.theta[self.Fsw], self.Fsw)
        tau_sw_right = self.create_transJ(self.theta[self.Bsw], self.Bsw)
        temp_F_sw = np.block([
            [tau_sw_left, np.zeros((3, 3))],
            [np.zeros((3, 3)), tau_sw_right]
        ])
        tau_sw = temp_F_sw @ F_sw

        if self.Fsw == FL:
            set_motor_torque(FL, 0, tau_sw[0][0])
            set_motor_torque(FL, 1, tau_sw[1][0])
            set_motor_torque(FL, 2, tau_sw[2][0])

            set_motor_torque(BR, 0, tau_sw[3][0])
            set_motor_torque(BR, 1, tau_sw[4][0])
            set_motor_torque(BR, 2, tau_sw[5][0])
        else:
            set_motor_torque(FR, 0, tau_sw[0][0])
            set_motor_torque(FR, 1, tau_sw[1][0])
            set_motor_torque(FR, 2, tau_sw[2][0])

            set_motor_torque(BL, 0, tau_sw[3][0])
            set_motor_torque(BL, 1, tau_sw[4][0])
            set_motor_torque(BL, 2, tau_sw[5][0])
        # print(tau)
        # print(tau_sw)

    def run(self):
        # 初始化webots节点设备
        webot_device_init()
        while robot.step(timestep) != -1:
            # 更新数据
            self.update_robot_state()
            self.control()


if __name__ == '__main__':
    q = Quadruped_robot_mix()
    q.run()
